#include "Tasks.h"
#include "adc_battery.h"
#include "IMU.h"
#include "uart.h"
#include "vl53lxx.h"
#include "optical_flow.h"
#include "data_deal.h"
#include "attitude_control.h"
#include "position_control.h"
#include "general_gpio.h"

/*全局变量*/
DroneFlightControl FlightControl;      //飞行器状态变量
DronePIDPara Para_Info;					//飞行器PID参数集合全局变量
DroneRTInfo RT_Info;                   //飞行器实时数据
DroneErrangle Errangle_Info;           //飞行器平地校准数据
DroneTargetInfo Target_Info;           //飞行器目标的全局变量
Controller Control_Info;               //手柄控制全局变量
Throttle Throttle_Info;                //油门全局变量

u8 StartFly=0; //标记飞行器是否开启四轴飞行

//任务同步使用的信号量
SemaphoreHandle_t AttitudeInner_Control;	
SemaphoreHandle_t IMU_Update;
SemaphoreHandle_t Data_Deal;


/**
* @Description IMU姿态解算任务 500hz
 */
void IMU_task(void *arg)
{
	IMU_Update=xSemaphoreCreateBinary();
	IMU_HardwareInit();
	while(1)
	{
		xSemaphoreTake(IMU_Update,portMAX_DELAY);
		IMU_Info_Update();
	}
}


/**
* @Description 超声波测距任务(已经不使用)
 */
void Ultra_task(void *arg)
{
	
	while(1)
	{
		
		ReceiveUltraData();
		vTaskDelay(50);
	}
}


/**
* @Description 激光测距任务 20hz
 */
void Laser_task(void *arg)
{
	
	while(1)
	{
		float result = vl53l1xMeasure();
		//数据大于0有效
		if(result>0)
			RT_Info.LASER_Alt = result;
		vTaskDelay(50);
	}
}

extern u16 vl53lxxId;
/**
* @Description 光流任务 100hz
 */
void OptionFlow_task(void *arg)
{
	if(vl53lxxId==0)
	{
		//初始化失败
		while(1){
			BEEP_ON;
			vTaskDelay(5);
			BEEP_OFF;
			vTaskDelay(5);
		}
	}
	vl53l1xSetParam();	
	while(1)
	{
		
		Horizontal_Update();
		vTaskDelay(10);
	}
}


/**
* @Description 电池电压任务	10hz
 */
void Battery_task(void *arg)
{
	 
	while(1)
	{
		RT_Info.batteryVoltage = Average_battery(ADC_Get_battery());
		/* 电压小于11.45V，led闪烁提示*/
		if(RT_Info.batteryVoltage<11.45f && (FlightControl.OnOff != Drone_On))
		{
			RT_Info.lowPowerFlag=1;
			ALLLED_ON;
			vTaskDelay(300);
			ALLLED_OFF;
			vTaskDelay(300);
		}else{
			//电压小于10.6时，降落
			if(RT_Info.batteryVoltage<10.6f)
			{
				FlightControl.landFlag=1;
			}
			RT_Info.lowPowerFlag=0;
		}
		vTaskDelay(100);
	}
}


/**
* @Description 串口通信数据解析任务
 */
void Data_Deal_task(void *arg)
{
	Data_Deal=xSemaphoreCreateBinary();
	_Data_Rx PC_Data;
	while(1)
	{
		xSemaphoreTake(Data_Deal,portMAX_DELAY);
		memcpy(PC_Data.buf,Bluetooth_rx.buf,sizeof(Bluetooth_rx.buf));
		PC_Data.len=Bluetooth_rx.len;
		data_deal(PC_Data);
	}
}


/**
 * @Description 数据汇报上位机任务 40hz
 */
void Data_Report_task(void *arg)
{
	while(1)
	{
		if(FlightControl.ReportSW==Report_SET)
		{
			Send_Para_to_PC();
			FlightControl.ReportSW=Report_RESET;
		}
		Send_IMU_to_PC();
		
		vTaskDelay(25);
	}
}


/**
* @Description 姿态内环控制任务 400hz
 */

void AttitudeInner_Control_task(void *arg)
{
	AttitudeInner_Control=xSemaphoreCreateBinary();
	static unsigned int Pre_Fly_Time = 0;
	while(1)
	{
		xSemaphoreTake(AttitudeInner_Control,portMAX_DELAY);
		//飞行器是否开启
		if(FlightControl.OnOff==Drone_On)
		{
			//四轴飞行模式
			if(FlightControl.droneMode==Drone_Mode_4Axis)
			{
				//固定油门预飞： 800*2ms=1600ms
				if(Pre_Fly_Time<800)
				{
					Pre_Fly_Time++;
					PWM_OUT(300,300,300,300);
				}
				else{
					StartFly=1;	//开始飞行，进入内环控制
					Attitude_Inner_Control();
					//防止翻机
					if(RT_Info.Pitch>=25 || RT_Info.Pitch<=-25 || RT_Info.Roll>=25 || RT_Info.Roll<=-25)
					{
						PWM_OUT(0,0,0,0);
						while(1);
					}
				}
				
			}
			else{
				//调试模式下，不用起飞
				Attitude_Inner_Control();
				StartFly=0;
			}
		}
		else
		{
			//关闭电源，关闭pwm
			Pre_Fly_Time=0;
			StartFly=0;
			PWM_OUT(0,0,0,0);
			
		}
			
	}
}


/**
* @Description 姿态外环控制任务 200hz
 */
void AttitudeOuter_Control_task(void *arg)
{
	
	while(1)
	{
		
		Attitude_Outer_Control();
		vTaskDelay(5);
	}
}


/**
* @Description 位置内环控制任务 250hz
 */
void PositionInner_Control_task(void *arg)
{
	
	while(1)
	{
		
		Position_Inner_Control();
		vTaskDelay(4);
	}
}
